/*
 * @Author: Author && Email
 * @Date: 2024-03-17 16:00:43
 * @LastEditors: error: error: git config user.name & please set dead value or install git && error: git config user.email & please set dead value or install git & please set dead value or install git
 * @LastEditTime: 2024-03-19 16:21:52
 * @FilePath: \MDK-ARMd:\WorkSpace\PRJ\Washing_machine\software\Washing_machine_V1\User\moudle\motor\motor_speed.c
 * @Description: 
 * 
 * Copyright (c) 2024 by ${git_name_email}, All Rights Reserved. 
 */
#include "motor_speed.h"

motor_speed g_drum_speed;

/**
 * @description: 电机速度控制相关变量初始化
 * @param {motor_speed} *_mspd_ptr
 * @param {uint16_t} _kp
 * @param {uint16_t} _ki
 * @return {*}
 */
void motor_speed_para_init(motor_speed *_mspd_ptr, uint16_t _kp, uint16_t _ki)
{
     memset(_mspd_ptr, 0, sizeof(motor_speed));
     _mspd_ptr->kp = _kp;
     _mspd_ptr->ki = _ki;
     _mspd_ptr->max_speed = SPEED_MAX;
     _mspd_ptr->min_speed = SPEED_MIN;
     _mspd_ptr->max_duty = DUTY_MAX;
     _mspd_ptr->min_duty = DUTY_MIN;
}

/**
 * @description: 电机速度PI控制器
 * @param {motor_speed} *_mspd_ptr
 * @return {*}
 */
void motor_speed_pi_controller(motor_speed *_mspd_ptr)
{
	int16_t temp1;
	int16_t temp2;
	
	/* 速度限制 */
	if(_mspd_ptr->target_speed >= SPEED_MAX)
	{
		_mspd_ptr->target_speed = SPEED_MAX;
	}
	else if(_mspd_ptr->target_speed <= SPEED_MIN)
	{
		_mspd_ptr->target_speed = SPEED_MIN;
	}
	_mspd_ptr->err = _mspd_ptr->target_speed - _mspd_ptr->feedback_speed; /* 当前误差 */
	_mspd_ptr->err_sum += _mspd_ptr->err;  /* 历史误差累加和 */
	/* 积分限幅 */
	if(_mspd_ptr->err_sum >= 50000)
	{
		_mspd_ptr->err_sum = 50000;
	}
	else if(_mspd_ptr->err_sum <= -50000)
	{
		_mspd_ptr->err_sum = -50000;
	}
	temp1 = _mspd_ptr->err * _mspd_ptr->kp / 100;
	if(temp1 == 0){
		if(_mspd_ptr->err > 0)
		{
			temp1 = 1;
		}
		else if(_mspd_ptr->err < 0)
		{
			temp1 = -1;
		}
	}
	temp2 = _mspd_ptr->err_sum * _mspd_ptr->ki / 1000;
	if(temp2 == 0){
		if(_mspd_ptr->err_sum > 0)
		{
			temp2 = 10;
		}
		else if(_mspd_ptr->err_sum < 0)
		{
			temp2 = -10;
		}
	}
	_mspd_ptr->duty = temp1 + temp2 ;
	/* 输出限幅 */
	if(_mspd_ptr->duty >= DUTY_MAX)
	{
		_mspd_ptr->duty = DUTY_MAX;
	}
	else if(_mspd_ptr->duty <= DUTY_MIN)
	{
		_mspd_ptr->duty = DUTY_MIN;
	}
}

/**
 * @description: 电机速度控制任务 1ms调用
 * @return {*}
 */
void motor_speed_task(void)
{
	static uint8_t ticker = 0;
	static uint8_t init_flag = 0;
	if(init_flag == 0)
	{
		motor_speed_para_init(&g_drum_speed, DRUM_MOTOR_KP, DRUM_MOTOR_KI);
		init_flag = 1;
	}
	else
	{
		ticker++;	
		if(ticker >= 100){		/* 电机速度控制 */
			if(g_drum_speed.target_speed == 0)
			{
				g_motor_drum.running = 0;
			}
			else
			{
				g_motor_drum.running = 1;
			}
			if (g_motor_drum.running == 1)
			{
				g_drum_speed.feedback_speed = g_encoder.speed;
				motor_speed_pi_controller(&g_drum_speed);
				g_motor_drum.duty = g_drum_speed.duty;
			}
			else
			{
				g_drum_speed.feedback_speed = 0;
				g_drum_speed.err_sum = 0;
			}
			ticker = 0;
		}
	}
}
